/*
 * time.c
 *
 *  Created on: 2022年11月18日
 *      Author: paper
 */
#include "time.h"
uint8 go_flag = 0;//开关标志位
uint8 run = 0;//开关标志位
uint8 yaw_flag=0;//转向标志位
float image_error = 0; //摄像头偏差(用于压弯)
float image_error1 = 0; //摄像头偏差(用于转向环)
extern float k;
uint8 rst_flag =1;
unsigned char Emergency_flag = 0;
float Balance_Pwm =0;
float Velocity_Pwm =0;
float speed_Pwm =0;
float Pwm=0;
float PWM_out=0;
Timer_0 Flag;
void Fuse_result(void) {
//    image_Error=40;
    // 1ms控制
        if (Flag.T_1ms == 1) {

            get_imu_data();//陀螺仪数据获取
            image_error= pinghua_error(image_Error,0.0450);//平滑误差使压弯转向更丝滑
            image_error1= LR_setSpin_retarder(image_error, 0.045);
//            Dynamic_zero=LR_angle_retarder(-image_error * 0.02, 0.01);//左右动态零点0.16  0.02
//            qianhou_zero=QH_angle_retarder(image_error * 0.3, 0.02);//前后动态零点
//            Dynamic_zero=0;
            Flag.T_1ms = 0;
        }
    // 2ms控制
    if (Flag.T_2ms == 1) {

        //陀螺仪姿态解算
        imu_task();

        //姿态异常急停
        if (go_flag == 1 && (eulerAngle.roll > 40 || eulerAngle.roll < -40||eulerAngle.pitch > 40 || eulerAngle.pitch < -40  )) {


            SET_wushua_PWM(0,0);
            SET_BDC_PWM(0);
            go_flag = 0;

        }
            if (go_flag == 1) {
            //左右平衡角速度环
            pidout_3f = ROLL_ANGLESPEED_LOOP(pidout_2f);
            pidout_6f = BLDC_YAW_ANGLESPEED_LOOP(pidout_5f);


            if(pidout_6f>2500)pidout_6f=2500;
            if(pidout_6f<-2500)pidout_6f=-2500;
//                 SET_wushua_PWM((int)(pidout_3f), (int)(-pidout_3f) );//设置左右无刷速度
                SET_wushua_PWM((int)(pidout_3f + pidout_6f), (int)(-(pidout_3f - pidout_6f))  );//设置左右无刷速度



            //前后平衡角速度环
            PWM_out = BDC_LOOP(Balance_Pwm,20*imu.gyro.gyro[Y]);


            SET_BDC_PWM((int)PWM_out);//设置有刷电机速度

        }

        Flag.T_2ms = 0;

    }

    // 5ms控制,按键扫描
    if (Flag.T_5ms == 1) {

        key_scanner();
               Flag.T_5ms = 0;
           }
    // 10ms控制
    if (Flag.T_10ms == 1) {

        seekfree_assistant_data_analysis();

                  k2 = -0.165;
                  k=0.000175;
                Dynamic_zero=k2*image_error;//动态零点
                ZHONGZHI_roll=eulerAngle.roll +Dynamic_zero;
                float Dynamic_zero1=fabs(image_error*k*encoder_bdc);
//                //压弯限幅
                if(Dynamic_zero<-Dynamic_zero1){
                    Dynamic_zero=-Dynamic_zero1;
                }
                if(Dynamic_zero>=Dynamic_zero1){
                    Dynamic_zero=Dynamic_zero1;
                }

        if (go_flag == 1) {
            //左右平衡角度环
            pidout_2f = ROLL_ANGLE_LOOP(pidout_1f);

            //yaw轴转向角度环,期望偏差为0，实际偏差为摄像头偏差
            pidout_5f = BLDC_YAW_ANGLE_LOOP(pidout_4f);


            //前后平衡角度环
            Balance_Pwm = BDC_BANLANCE_LOOP(Velocity_Pwm,-eulerAngle.pitch);

        }
        Flag.T_10ms = 0;
    }
    // 20ms控制  左右平衡飞轮速度环 按键扫描 编码器获取
    if (Flag.T_20ms == 1) {
        //编码器角度获取
        encoder_get();

        //Key_scan();
        if (go_flag == 1) {
            //左右平衡速度环
            pidout_1f = ROLL_SPEED_LOOP(0);

            //yaw轴转向速度环
            pidout_4f = BLDC_YAW_SPEED_LOOP(0);

            //前后平衡速度环
            Velocity_Pwm = BDC_SPEED_LOOP(bdc_speed ,encoder_bdc );

        }

        Flag.T_20ms = 0;
    }

}
//平滑误差1ms控制
float pinghua_error(float input,float unit){
    static float output=0;
//    if(input*unit<0)unit=unit*0.3;
    if(unit==0){
        output=input;
        return input;
    }
    if(output>input){//如果输入减小，则减少output
        output-=unit;//减少一个单位
        if(output<input)output=input;//如果超调，则限制output
    }
    if(output<input){//如果输入增加，则增加output
        output+=unit;//增加一个单位
        if(output>input)output=input;//如果超调，则限制output
    }
    return output;
}
float LR_setSpin_retarder(float input, float unit)
{
    static float output = 0;
//        if (input * unit < 0)
//            unit = unit * 0.3;
        if (unit == 0)
        {
            output = input;
            return input;
        }
        if (output > input) // 如果输入减小，则减少output
        {
            output -= unit; // 减少一个单位
            if (output < input)
                output = input; // 如果超调，则限制output
        }
        if (output < input) // 如果输入增加，则增加output
        {
            output += unit; // 增加一个单位
            if (output > input)
                output = input; // 如果超调，则限制output
        }
        return output;
}
// 动量轮 角度缓慢变化
float LR_angle_retarder(float input, float unit)
{
    static float output = 0;
    if (input * unit < 0)
        unit = unit * 0.3;
    if (unit == 0)
    {
        output = input;
        return input;
    }
    if (output > input) // 如果输入减小，则减少output
    {
        output -= unit; // 减少一个单位
        if (output < input)
            output = input; // 如果超调，则限制output
    }
    if (output < input) // 如果输入增加，则增加output
    {
        output += unit; // 增加一个单位
        if (output > input)
            output = input; // 如果超调，则限制output
    }
    return output;
}
float QH_angle_retarder(float input, float unit)
{
    static float output = 0;
    if (input * unit < 0)
        unit = unit * 0.3;
    if (unit == 0)
    {
        output = input;
        return input;
    }
    if (output > input) // 如果输入减小，则减少output
    {
        output -= unit; // 减少一个单位
        if (output < input)
            output = input; // 如果超调，则限制output
    }
    if (output < input) // 如果输入增加，则增加output
    {
        output += unit; // 增加一个单位
        if (output > input)
            output = input; // 如果超调，则限制output
    }
    return output;
}
